
#include "stdio.h"
#include "math.h"
#include "CGPointExtension.h"

#define kCGPointEpsilon FLT_EPSILON

CGFloat
ccpLength(const CGPoint v)
{
    return sqrtf(ccpLengthSQ(v));
}

CGFloat
ccpDistance(const CGPoint v1, const CGPoint v2)
{
    return ccpLength(ccpSub(v1, v2));
}

CGPoint
ccpNormalize(const CGPoint v)
{
    return ccpMult(v, 1.0f/ccpLength(v));
}

CGPoint
ccpForAngle(const CGFloat a)
{
    return ccp(cosf(a), sinf(a));
}

CGFloat
ccpToAngle(const CGPoint v)
{
    return atan2f(v.y, v.x);
}

CGPoint ccpLerp(CGPoint a, CGPoint b, float alpha)
{
    return ccpAdd(ccpMult(a, 1.f - alpha), ccpMult(b, alpha));
}

float clampf(float value, float min_inclusive, float max_inclusive)
{
    if (min_inclusive > max_inclusive) {
        float tmp=min_inclusive;
        min_inclusive = max_inclusive;
        max_inclusive = tmp;
    }
    return value < min_inclusive ? min_inclusive : value < max_inclusive? value : max_inclusive;
}

CGPoint ccpClamp(CGPoint p, CGPoint min_inclusive, CGPoint max_inclusive)
{
    return ccp(clampf(p.x,min_inclusive.x,max_inclusive.x), clampf(p.y, min_inclusive.y, max_inclusive.y));
}

CGPoint ccpFromSize(CGSize s)
{
    return ccp(s.width, s.height);
}

CGPoint ccpCompOp(CGPoint p, float (*opFunc)(float))
{
    return ccp(opFunc(p.x), opFunc(p.y));
}

BOOL ccpFuzzyEqual(CGPoint a, CGPoint b, float var)
{
    if(a.x - var <= b.x && b.x <= a.x + var)
        if(a.y - var <= b.y && b.y <= a.y + var)
            return true;
    return false;
}

CGPoint ccpCompMult(CGPoint a, CGPoint b)
{
    return ccp(a.x * b.x, a.y * b.y);
}

float ccpAngleSigned(CGPoint a, CGPoint b)
{
    CGPoint a2 = ccpNormalize(a);
    CGPoint b2 = ccpNormalize(b);
    float angle = atan2f(a2.x * b2.y - a2.y * b2.x, ccpDot(a2, b2));
    if( fabs(angle) < kCGPointEpsilon ) return 0.f;
    return angle;
}

CGPoint ccpRotateByAngle(CGPoint v, CGPoint pivot, float angle)
{
    CGPoint r = ccpSub(v, pivot);
    float cosa = cosf(angle), sina = sinf(angle);
    float t = r.x;
    r.x = t*cosa - r.y*sina + pivot.x;
    r.y = t*sina + r.y*cosa + pivot.y;
    return r;
}


BOOL ccpSegmentIntersect(CGPoint A, CGPoint B, CGPoint C, CGPoint D)
{
    float S, T;
    
    if( ccpLineIntersect(A, B, C, D, &S, &T )
       && (S >= 0.0f && S <= 1.0f && T >= 0.0f && T <= 1.0f) )
        return YES;
    
    return NO;
}

CGPoint ccpIntersectPoint(CGPoint A, CGPoint B, CGPoint C, CGPoint D)
{
    float S, T;
    
    if( ccpLineIntersect(A, B, C, D, &S, &T) ) {
        // Point of intersection
        CGPoint P;
        P.x = A.x + S * (B.x - A.x);
        P.y = A.y + S * (B.y - A.y);
        return P;
    }
    
    return CGPointZero;
}

BOOL ccpLineIntersect(CGPoint A, CGPoint B,
                      CGPoint C, CGPoint D,
                      float *S, float *T)
{
    // FAIL: Line undefined
    if ( (A.x==B.x && A.y==B.y) || (C.x==D.x && C.y==D.y) ) return NO;
    
    const float BAx = B.x - A.x;
    const float BAy = B.y - A.y;
    const float DCx = D.x - C.x;
    const float DCy = D.y - C.y;
    const float ACx = A.x - C.x;
    const float ACy = A.y - C.y;
    
    const float denom = DCy*BAx - DCx*BAy;
    
    *S = DCx*ACy - DCy*ACx;
    *T = BAx*ACy - BAy*ACx;
    
    if (denom == 0) {
        if (*S == 0 || *T == 0) {
            // Lines incident
            return YES;
        }
        // Lines parallel and not incident
        return NO;
    }
    
    *S = *S / denom;
    *T = *T / denom;
    
    // Point of intersection
    // CGPoint P;
    // P.x = A.x + *S * (B.x - A.x);
    // P.y = A.y + *S * (B.y - A.y);
    
    return YES;
}

float ccpAngle(CGPoint a, CGPoint b)
{
    float angle = acosf(ccpDot(ccpNormalize(a), ccpNormalize(b)));
    if( fabs(angle) < kCGPointEpsilon ) return 0.f;
    return angle;
}